/*
 * demo.cpp
 *
 *  Created on: Oct 24, 2012
 *      Author: ashok
 */



#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/highgui/highgui.hpp>
#include "opencv2/core/core.hpp"

#include <iostream>
#include <math.h>
#include <string.h>
using namespace cv;
using namespace std;


static void help()
{
    cout <<
    "\nA program using pyramid scaling, Canny, contours, contour simpification and\n"
    "memory storage (it's got it all folks) to find\n"
    "squares in a list of images pic1-6.png\n"
    "Returns sequence of squares detected on the image.\n"
    "the sequence is stored in the specified memory storage\n"
    "Call:\n"
    "./squares\n"
    "Using OpenCV version %s\n" << CV_VERSION << "\n" << endl;
}


int thresh = 50, N = 11;
const char* wndname = "Square Detection Demo";

// helper function:
// finds a cosine of angle between vectors
// from pt0->pt1 and from pt0->pt2
static double angle( Point pt1, Point pt2, Point pt0 )
{
    double dx1 = pt1.x - pt0.x;
    double dy1 = pt1.y - pt0.y;
    double dx2 = pt2.x - pt0.x;
    double dy2 = pt2.y - pt0.y;
    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

// returns sequence of squares detected on the image.
// the sequence is stored in the specified memory storage
static void findSquares( const Mat& image, vector<vector<Point> >& squares )
{
    squares.clear();

    Mat pyr, timg, gray0(image.size(), CV_8U), gray;

    // down-scale and upscale the image to filter out the noise
    pyrDown(image, pyr, Size(image.cols/2, image.rows/2));
    pyrUp(pyr, timg, image.size());
    vector<vector<Point> > contours;

    // find squares in every color plane of the image
    for( int c = 0; c < 3; c++ )
    {
        int ch[] = {c, 0};
        mixChannels(&timg, 1, &gray0, 1, ch, 1);

        // try several threshold levels
        for( int l = 0; l < N; l++ )
        {
            // hack: use Canny instead of zero threshold level.
            // Canny helps to catch squares with gradient shading
            if( l == 0 )
            {
                // apply Canny. Take the upper threshold from slider
                // and set the lower to 0 (which forces edges merging)
                Canny(gray0, gray, 0, thresh, 5);
                // dilate canny output to remove potential
                // holes between edge segments
                dilate(gray, gray, Mat(), Point(-1,-1));
            }
            else
            {
                // apply threshold if l!=0:
                //     tgray(x,y) = gray(x,y) < (l+1)*255/N ? 255 : 0
                gray = gray0 >= (l+1)*255/N;
            }

            // find contours and store them all as a list
            findContours(gray, contours, CV_RETR_LIST, CV_CHAIN_APPROX_SIMPLE);

            vector<Point> approx;

            // test each contour
            for( size_t i = 0; i < contours.size(); i++ )
            {
                // approximate contour with accuracy proportional
                // to the contour perimeter
                approxPolyDP(Mat(contours[i]), approx, arcLength(Mat(contours[i]), true)*0.03, true);

                // square contours should have 4 vertices after approximation
                // relatively large area (to filter out noisy contours)
                // and be convex.
                // Note: absolute value of an area is used because
                // area may be positive or negative - in accordance with the
                // contour orientation
                if( approx.size() == 4 &&
                    fabs(contourArea(Mat(approx))) > 1000 &&
                    isContourConvex(Mat(approx)) )
                {
                    double maxCosine = 0;

                    for( int j = 2; j < 5; j++ )
                    {
                        // find the maximum cosine of the angle between joint edges
                        double cosine = fabs(angle(approx[j%4], approx[j-2], approx[j-1]));
                        maxCosine = MAX(maxCosine, cosine);
                    }

                    // if cosines of all angles are small
                    // (all angles are ~90 degree) then write quandrange
                    // vertices to resultant sequence
                    if( maxCosine < 0.3 )
                        squares.push_back(approx);
                }
            }
        }
    }
}


// the function draws all the squares in the image
static void drawSquares( Mat& image, const vector<vector<Point> >& squares )
{
    for( size_t i = 0; i < squares.size(); i++ )
    {
        const Point* p = &squares[i][0];
        int n = (int)squares[i].size();
        polylines(image, &p, &n, 1, true, Scalar(0,255,0), 3, CV_AA);
    }
    if(squares.size() != 0)
    {
    	putText(image, "PRESENT", cvPoint(30, 30), FONT_HERSHEY_SIMPLEX,
    	    			 1.0, cvScalar(0,255,0), 4, CV_AA);
    }
    else
    {
    	putText(image, "ABSENT", cvPoint(30, 30), FONT_HERSHEY_SIMPLEX,
    	    			 1.0, cvScalar(0,0,255), 4, CV_AA);
    }
    imshow(wndname, image);
}

int redBoxHSV[6] = {0,36,82,209,25,195};
int blankHSV[6] = {0,255,0,255,0,255};

int main(int argc, char** argv)
{
	VideoCapture cap(0); // open the default camera
	if(!cap.isOpened())  // check if we succeeded
		return -1;
	//Mat src;
	Mat src = imread(argv[2]);
	cap >> src;
	int* hsvConfig = blankHSV;
    if (src.empty())
        return -1;
    namedWindow("Video", 1);
    namedWindow("Tracking", 1);
	int hMin, hMax, sMin, sMax, vMin, vMax;
	hMin = 113;
	hMax = 145;
	sMin = 111;
	sMax = 255;
	vMin = 172;
	vMax = 255;

	createTrackbar("Hue Min", "Tracking", &hMin, 255);
	createTrackbar("Hue Max", "Tracking", &hMax, 255);
	createTrackbar("Sat Min", "Tracking", &sMin, 255);
	createTrackbar("Sat Max", "Tracking", &sMax, 255);
	createTrackbar("Val Min", "Tracking", &vMin, 255);
	createTrackbar("Val MaX", "Tracking", &vMax, 255);
//    createTrackbar("Hue Min", "Tracking", &hsvConfig[0], 255);
//    createTrackbar("Hue Max", "Tracking", &hsvConfig[1], 255);
//    createTrackbar("Sat Min", "Tracking", &hsvConfig[2], 255);
//    createTrackbar("Sat Max", "Tracking", &hsvConfig[3], 255);
//    createTrackbar("Val Min", "Tracking", &hsvConfig[4], 255);
//    createTrackbar("Val MaX", "Tracking", &hsvConfig[5], 255);
    for(;;)
    {
    cap >> src;
    Mat hsv;
    cvtColor(src, hsv, CV_BGR2HSV);

    Mat bw;
    //inRange(hsv, Scalar(hsvConfig[0], hsvConfig[1], hsvConfig[2]),
    //		Scalar(hsvConfig[3], hsvConfig[4], hsvConfig[5]), bw);
    inRange(hsv, Scalar(hMin, sMin, vMin), Scalar(hMax, sMax, vMax), bw);

    vector<vector<Point> > contours;
    findContours(bw.clone(), contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);

    Mat dst = Mat::zeros(src.size(), src.type());
    drawContours(dst, contours, -1, Scalar::all(255), CV_FILLED);

    dst &= src;
    //imshow("video", src);
    imshow("Tracking", dst);
    vector<vector<Point> > squares;
    findSquares(dst, squares);
    drawSquares(src, squares);


    if(waitKey(10)>=0)	{break;}
    }
    return 0;
}
